#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <fstream>

#include <thread>

#include "mat_utils.h"


void print_mat2d(cv::Mat outmat, std::string fname, std::string delimiter) {
	std::ofstream outfile;
	outfile.open(fname, std::ios::out);
	for (int i=0;i<outmat.rows;i++) {
		for (int j=0;j<outmat.cols;j++) {
			outfile << outmat.at<double>(i,j) << delimiter;
		}
		outfile<<std::endl;
	}
	outfile.close();
}

void print_snake(cv::Mat snake, std::string delimiter, std::ostream& pipe)
{
	for (int i=0;i<snake.rows;i++) {
		pipe << snake.at<double>(i,0) << delimiter << snake.at<double>(i,1) <<std::endl;
	}
}

void print_snake(cv::Mat snake, std::string delimiter, std::string fname)
{
	std::ofstream outfile;
	outfile.open(fname, std::ios::out);
	for (int i=0;i<snake.rows;i++) {
		outfile << snake.at<double>(i,0) << delimiter << snake.at<double>(i,1) <<std::endl;
	}
	outfile.close();
}

//TEMPORARY, dangerous even, this will modify Mat internals, so care
//deletes numpts of indices in the snake starting from the beginning
//remember:
//		+----------------------
//		| 0 | 1 | 2 | 3 |....
//		+----------------------
//----------------------+
//	 ...|n-4|n-3|n-2|n-1|
//----------------------+
void fix_snake(cv::Mat& inmat, int numpts)
{
	for (int i=0;i<numpts;i++) {
		int ndx = 0;
		while (ndx < inmat.rows-1) {
			inmat.at<double>(ndx,0) = inmat.at<double>(ndx+1,0);
			inmat.at<double>(ndx,1) = inmat.at<double>(ndx+1,1);
			ndx++;
		}
		inmat.pop_back();
	}
}

//Loops through a 2d matrix and if abs(entry) < precision
//sets it to 0
void zero_mat2d(cv::Mat& inmat, double precision)
{
	for (int i=0;i<inmat.rows;i++) {
		for (int j=0;j<inmat.cols;j++) {
			if (abs(inmat.at<double>(i,j)) < precision) {
				inmat.at<double>(i,j) = 0;
			}
		}
	}
}

//Thread function that displays image for a bit
//somewhat temporary
int __stdcall dispImg(imgData* data) {
	//return 0;

	cv::namedWindow(data->winName, CV_WINDOW_NORMAL);
	cv::imshow(data->winName, (data->imgsrc));
	cv::waitKey(300000);
	free(data);
	return 0;
}

//TODO Rewrite this function
	//Remap Mat to higher res Mat (possibly trichannel Mat too)
	//Divide Mat into multiple quadrants
	//Calculate vector for that quadrant
	//Create arrow for that quadrant (over image?)

//Draws a vector flow 
int draw_flow_image(cv::Mat& vx, cv::Mat& vy, cv::Mat& pic,
                    CvScalar back_color, CvScalar fore_color, float thresh)
{

    int hsize=3;//size of the arrow head

    int width, height;
    int i,j;

    width=vx.cols,height=vx.rows;

    float * px, *py;

    px=(float *)vx.data;
	py=(float *)vy.data;

#define VX(x,y) px[(y)*width+(x)]
#define VY(x,y) py[(y)*width+(x)]

    if(thresh<1)
        thresh=0.8f;

    float thresh1=thresh*thresh;

    float maxx=-1,maxy=-1;

    for(j=0;j<height;j++)
    {
        for(i=0;i<width;i++)
        {
            float dx,dy;
            dx=VX(i,j),dy=VY(i,j);

            if(fabsf(dx)>fabsf(maxx))
                maxx=dx;

            if(fabsf(dy)>fabsf(maxy))
                maxy=dy;

            float ds=dx*dx+dy*dy;
            if(thresh>0.5)//if there's a threshold on the magnitude of velocity
            {
                if(ds<thresh1)//don't bother to draw if it's too small
                    continue;
            }
           
            int ex,ey;
            ex=cvRound(i+dx);
            ey=cvRound(j+dy);

            //draw the arrow body
			//cv::line(pic,cvPoint(i,j),cvPoint(ex,ey),fore_color);

            //continue;
            //draw arrow head
            ds=sqrtf(ds);
            dx/=ds,dy/=ds;
            dx=-dx,dy=-dy;

            float ddx,ddy;
            ddx=-dy,ddy=dx;
            ddx+=dx,ddy+=dy;
            ddx*=hsize,ddy*=hsize;

            //cv::line(pic,cvPoint(ex,ey),cvPoint(cvRound(ex+ddx),cvRound(ey+ddy)),fore_color);

            ddx=dy,ddy=-dx;
            ddx+=dx,ddy+=dy;
            ddx*=hsize,ddy*=hsize;

            //cv::line(pic,cvPoint(ex,ey),cvPoint(cvRound(ex+ddx),cvRound(ey+ddy)),fore_color);
        }
    }
	
	cv::line(pic,cvPoint(8,8),cvPoint(40,40),fore_color);

//results in a race condition since id gets deallocated
	imgData *id = new imgData(std::string("vecfield"), pic);
	std::thread dispThread(dispImg, id);
	dispThread.detach();
	cv::waitKey(3000);
	return 0;
}